#ifndef ASTAR_PLANNER_HPP
#define ASTAR_PLANNER_HPP

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"

#include "nav2_core/global_planner.hpp"
#include "nav_msgs/msg/path.hpp"

#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/point.hpp"

#include "robot_one_navigation/A_Star/Astar.hpp"

namespace astar_planner
{
    class AstarPlanner : public nav2_core::GlobalPlanner
    {
    private:
        nav2_util::LifecycleNode::SharedPtr node_;
        std::string name_, global_frame_;
        std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
        nav2_costmap_2d::Costmap2D *cost_map_;
        Astar astar_;

        rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_vis_map_;

    public:
        AstarPlanner() = default;
        ~AstarPlanner() = default;

        void configure(
            const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
            std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
            std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;

        void cleanup() override;

        void activate() override;

        void deactivate() override;

        nav_msgs::msg::Path createPlan(
            const geometry_msgs::msg::PoseStamped &start,
            const geometry_msgs::msg::PoseStamped &goal) override;

        // 将 costmap 栅格地图中的二维栅格坐标（行列索引）转换为世界坐标系下的三维位姿（Pose）
        geometry_msgs::msg::Pose get_world_coords(const Eigen::Vector2i &pose, const nav2_costmap_2d::Costmap2D *costmap);
    };

} // namespace astar_planner

#endif // ASTAR_PLANNER_HPP